package roborally;

import be.kuleuven.cs.som.annotate.*;

/**
 * This is a class of Robots which have a position and orientation on a board and an amount of energy. Since the maximum amount of energy for each 
 * Robot could change in the future, the amount of energy required to turn, the maximum amount of energy and the amount of energy needed to move 
 * are also ascribed to Robots. 
 * 
 * @note 	No setters are introduced for the position and orientation to oblige using methods move(), 
 * 			turnClockwise() and turnCounterClockwise()
 * @note 	In each constructor or method, all aspects related to orientation are worked out totally, all 
 * 			aspects related to position defensively and all aspects related to energy nominally.
 * @note 	Nominal aspects are specified with preconditions and assert-statements which are not
 * 			meant to be used dynamically
 * @note 	Methods are ordered in increasing complexity
 * @note 	All public methods are specified both formally and informally
 * @note 	Annotation @Raw is not used since Robots always satisfy their invariants
 * @note 	Assert-statements that are not executed and some unreachable statements
 * 			or incompatible boolean expressions still give a code coverage of +-90%
 * 
 * @invar	The position is valid
 * 			|  isValidPosition(this.getX,this.getY)
 * @invar	The energy is valid
 * 			|  isValidEnergy(this.getEnergy(), this.getMaxEnergy() ) 
 * @invar  	The orientation follows the convention of beeing in [0,1,2,3]
 * 			| 0 <= getOrientation() <= 3
 * @invar  	The maximum energy is strictly positive
 * 			|  getMaxEnergy() > 0 
 * @invar  	The energy required to turn is positive
 * 			|  getTurnEnergy()  >= 0 
 * @invar 	The energy required to move is positive
 * 			|  getMoveEnergy() >= 0
 * 
 * @version 1.0
 * @author 	Bart Opsomer 2de Bachelor Ingenieurswetenschappen Electrotechniek - Computerwetenschappen
 * 			Peter Opsomer 1ste Master Wiskundige IngenieursTechnieken
 */
public class Robot implements IRobot {
	
	// the orientation of the Robot: 0, 1, 2, 3 respectively represent up, right, down and left.
	private int orientation;
	// the x-coordinate of the Robot
	private long x;
	// the y-coordinate of the Robot
	private long y;
	// the energy of the Robot
	private double energy;
	// The maximum amount of energy this Robot can have, is no class variable because it has
	// to be adjustable for each object afterwards
	private final double maxEnergy; // = 20000;
	// The amount of energy needed to turn
	private final double turnEnergy; // = 100; Commented, otherwise impossible to do in constructor
	// The amount of energy needed to move forward
	private final double moveEnergy; // = 500;	

	// The maximum x-position of the Robot, static so that the same for all Robots
	private static final long maxX = Long.MAX_VALUE;
	// The maximum y-position of the Robot, static so that the same for all Robots
	private static final long maxY = Long.MAX_VALUE;
	
	/**
	 * Construct a new Robot with a given position, orientation and amount of energy. Other properties 
	 * are left default.
	 * 
	 * @pre 	energy is a valid amount of energy
	 * 			|  isValidEnergy(energy,20000.0) 
	 * 
	 * @param 	x the x-coordinate for the new Robot
	 * @param 	y the y-coordinate for the new Robot
	 * @param 	orientation the orientation of the new Robot: 0, 1, 2, 3 respectively represent 
	 * 			up, right, down and left.
	 * @param 	energy the (initial) energy for the Robot
	 * 
	 * @effect 	The more extended constructor is called with default arguments
	 * 		   	| this(x,y,orientation,energy,20000.0,100.0,500.0);
	 */
	@Raw
	public Robot (long x, long y, int orientation, double energy) throws IllegalArgumentException{
		this(x,y,orientation,energy,20000.0,100.0,500.0);
	}
	/**
	 *  Construct a new Robot with a given position, orientation, amount of energy, maximum amount of
	 *  energy, energy required to turn and move. 
	 * 
	 * @pre  	energy is a valid amount of energy
	 * 			|  isValidEnergy(energy,maxEnergy) 
	 * @pre  	The maximum energy is strictly positive
	 * 			|  maxEnergy > 0 
	 * @pre  	The energy required to turn is positive
   	 * 			| turnEnergy  >= 0 
 	 * @pre 	The energy required to move is positive
	 * 			| moveEnergy >= 0
	 * 
	 * @param 	x the x-coordinate for the new Robot
	 * @param 	y the y-coordinate for the new Robot
	 * @param 	orientation the orientation of the new Robot: 0, 1, 2, 3 respectively represent 
	 * 			up, right, down and left.
	 * @param 	energy the (initial) energy for the Robot
	 * 
	 * @post 	The x-coordinate is set
	 * 			| new.getX() == x
	 * @post 	The y-coordinate is set
	 * 			| new.getY() == y
	 * @post 	The orientation is set and uses a rollover/wrap so that the orientation is always contained 
	 * 		 	within (0,1,2,3) 
	 * 		 	|  new.getOrientation() == abs(orientation%4) 
	 * @post 	The energy is set
	 * 		 	| new.getEnergy() == energy
	 * @post 	The maximum energy is set
	 * 		 	| new.getMaxEnergy() == maxEnergy
	 * @post 	The amount of energy required to turn is set
	 * 		 	| new.getTurnEnergy() == turnEnergy
	 * @post 	The amount of energy required to move is set
	 * 		 	| new.getMoveEnergy() == moveEnergy
	 * 
	 * @throws 	IllegalArgumentException if an invalid position was given
	 * 		   	| !isValidPosition(x,y)
	 */
	@Raw
	public Robot (long x, long y, int orientation, double energy, double maxEnergy, 
			double turnEnergy, double moveEnergy) throws IllegalArgumentException{
		assert(isValidEnergy(energy,maxEnergy));
		assert((maxEnergy > 0 && turnEnergy >= 0) && moveEnergy >= 0);
	 	if (!isValidPosition(x,y) )
			throw new IllegalArgumentException("Position out of range.");
		this.x = x;
		this.y = y;
		this.orientation = Math.abs(orientation%4);
		// Energy should be OK because of precondition
		this.energy = energy;
		this.maxEnergy = maxEnergy;
		this.turnEnergy = turnEnergy;
		this.moveEnergy = moveEnergy;
	}
	
	/**
	 * Test whether the amount of energy is valid.
	 * @param 	energy The energy to test
	 * @param 	maxEnergy The maximum energy
	 * @return 	whether the energy is valid: positive and below maxEnergy
	 * 			|  (energy >= 0) && (energy <= maxEnergy)
	 */
	public static boolean isValidEnergy(double energy,double maxEnergy) {
		return (energy >= 0) && (energy <= maxEnergy);
	}
	/**
	 * Test whether (x,y) is a valid position of the Robot.
	 * @param 	x the supposed x-coordinate of the Robot
	 * @param 	y the supposed y-coordinate of the Robot
	 * @return 	whether the given coordinates form a valid position for the Robot
	 * 			| (x >= 0) && (x <= getMaxX() ) && (y >= 0) && (y <= getMaxY() )
	 */
	private static boolean isValidPosition(long x,long y) {
		return (x >= 0) && (x <= getMaxX() ) && (y >= 0) && (y <= getMaxY()) ;
	}
	/**
	 * @return the maximum x-position of the Robot
	 */
	@Immutable @Basic
	public static long getMaxX() {
		return maxX;
	}
	/**
	 * @return the maximum y-position of the Robot
	 */
	@Immutable @Basic
	public static long getMaxY() {
		return maxY;
	}

	/**
	 * @return 	orientation the orientation of the new Robot: 0, 1, 2, 3 respectively represent 
	 * 			up, right, down and left. 4,5,6,7 would also represent these by using Math.abs(
	 * 			getOrientation() % 4) to convert into [0 .. 3], but the convention has been made to 
	 * 			limit the orientation to that interval
	 */
	@Basic
	public int getOrientation() {
		return this.orientation;
	}
	/**
	 * @return 	the x-coordinate of the Robot
	 */
	@Basic
	public long getX() {
		return this.x;
	}
	/**
	 * @return the y-coordinate of the Robot
	 */
	@Basic
	public long getY() {
		return this.y;
	}
	/**
	 * @return the energy of this Robot
	 */
	@Basic
	public double getEnergy() {
		return this.energy;
	}
	/**
	 * @return the maximum amount of energy this Robot can have
	 */
	@Immutable @Basic
	public double getMaxEnergy() {
		return this.maxEnergy;
	}
	/**
	 * @return the amount of energy needed to turn
	 */
	@Immutable @Basic
	private double getTurnEnergy() {
		return turnEnergy;
	}
	/**
	 * @return the amount of energy needed to move one step forward
	 */
	@Immutable @Basic
	private double getMoveEnergy() {
		return moveEnergy;
	}
	/**
	 * Returns the relative amount of energy
	 * @return 	the fraction of energy that is left
	 * 			| getEnergy()/getMaxEnergy()
	 */
	public double getRelativeEnergy()
	{
		// can't divide by 0 because of invariant getMaxEnergy() > 0
		return getEnergy()/getMaxEnergy();
	}
	/**
	 * Recharge the robot's energy with the given amount
	 * @pre 	amount >= 0 
	 * @pre		getMaxEnergy() >= getEnergy() + amount
	 * @param 	amount the amount of energy to recharge the robot with
	 * @post 	The energy of the robot is increased with amount Wattsecond
	 * 			|  new.getEnergy() == this.getEnergy() + amount
	 */
	public void recharge(double amount) {
		assert(amount >= 0);
		assert(getMaxEnergy() >= getEnergy() + amount);
		this.energy += amount;
		//energy = Math.min(maxEnergy,energy + amount);
	}
	/**
	 * The robots energy is decreased with the given amount
	 * 
	 * @pre 	The Robot has at least the amount as energy
	 * 			| getEnergy() - amount >= 0 
	 * @param 	amount the amount of energy the robot uses
	 * @post 	The energy of the robot is decreased with amount Wattsecond
	 * 			|  new.getEnergy() == this.getEnergy() - amount
	 */
	public void useEnergy(double amount) {
		assert( getEnergy() - amount >= 0);
		this.energy = this.getEnergy() - amount;
	}

	/**
	 * Turn this Robot clockwise. 
	 * @pre 	The Robot has sufficient energy to rotate
	 * 			| this.getEnergy() >= this.getTurnEnergy()
	 * @post 	The Robot has turned clockwise 
	 * 			| new.getOrientation() == (this.getOrientation() + 1) % 4
	 * @effect 	The Robot has lost the energy required to turn
	 * 			| this.useEnergy(this.getTurnEnergy() )
	 */
	public void turnClockwise(){
		assert(this.getEnergy() >= this.getTurnEnergy() );
		// orientation cannot become negative with this
		this.orientation = (this.orientation + 1) % 4;
		useEnergy(getTurnEnergy() );
	}
	/**
	 * Turn this Robot counterclockwise.
	 * @pre 	The Robot has sufficient energy to rotate
	 * 			| this.getEnergy() >= this.getTurnEnergy()
	 * @post 	The Robot has turned counterclockwise 
	 * 			| new.getOrientation() == (this.getOrientation() + 3) % 4
	 * @effect 	The Robot has lost the energy required to turn
	 * 			| this.useEnergy(this.getTurnEnergy() )

	 */
	public void turnCounterClockwise(){
		assert(this.getEnergy() >= this.getTurnEnergy());
		// orientation = (orientation + 3) % 4 to counter negative orientations ex. (0-1)%4 = -1
		this.orientation = (this.orientation + 3) % 4;
		useEnergy(getTurnEnergy() );
	}

	/**
	 * Move this Robot one step forward.
	 * @pre 	The Robot has sufficient energy to move
	 * 			| this.getEnergy() >= this.getMoveEnergy()
	 * @post 	The Robot has moved forward one step
	 * 			| if getOrientation() == 0 then new.getY() == this.getY() - 1
	 * 			| if getOrientation() == 1 then new.getX() == this.getX() + 1
	 * 			| if getOrientation() == 2 then new.getY() == this.getY() + 1
	 * 			| if getOrientation() == 3 then new.getX() == this.getX() - 1
	 * @effect 	The Robot has lost the energy required to move
	 * 			| this.useEnergy(this.getMoveEnergy() )
	 * @throws 	MoveOutOfBoardException if the Robot would move out of the Board
	 * 			| getOrientation() == 0 && getY() == 0
	 * 			| getOrientation() == 1 && getX() == getMaxX()
	 * 			| getOrientation() == 2 && getY() == getMaxY()
	 * 			| getOrientation() == 3 && getX() == 0
	 */
	public void move() throws MoveOutOfBoardException {
		assert(this.getEnergy() >= this.getMoveEnergy());
		if ( 	(orientation == 0 && getY() == 0) || (orientation == 1 && getX() == getMaxX() )
				|| ( orientation == 2 && getY() == getMaxY() ) || (orientation == 3 && getX() == 0 ) )
			throw new MoveOutOfBoardException(this);
		switch (orientation) {
		case 0: y --;
				break;
		case 1: x ++;
		 		break;
		case 2: y ++;
		    	break;
		case 3: x --;
		 		break;
		}
		useEnergy(getMoveEnergy() );
	}

	/**
	 * Calculate the the minimal energy that is needed to reach point (x,y) from the current position of the robot 
	 * assuming a turn (in any direction) costs getTurnEnergy() and a move forward one step costs
	 * getMoveEnergy().
	 * 
	 * @return 	the minimal energy that is needed to reach point (x,y) , which is the Manhattan distance to the desired
	 * 			location times the energy needed to move one time plus the number of turns that are needed times the 
	 * 			energy required to turn. 
	 * 			When one coordinate is the same as the current coordinate of this already, 
	 * 			numberOfTurns = 0 if robot faces (x,y), 
	 * 			numberOfTurns = 2 if faced in the opposite direction and 
	 * 			numberOfTurns = 1 otherwise. 
	 * 			| let
	 * 			|	 if x == getX() then 
	 * 			|		if (y-getY()>0 && getOrientation() == 2) || (y-getY()>0 && getOrientation() == 0) || y == getY()
	 * 			|			numberOfTurns = 0
	 * 			|		else if (y-getY()>0 && getOrientation() == 0) || (y-getY()>0 && getOrientation() == 2)
	 * 			|			numberOfTurns = 2
	 * 			|		else
	 * 			|			numberOfTurns = 1
	 * 			|	 else if y == getY() then 
	 * 			|		if (x-getX()>0 && getOrientation() == 1) || (x-getX()<0 && getOrientation()  == 3) //|| x == getX()
	 * 			|			numberOfTurns = 0
	 * 			|		else if (x-getX()>0 && getOrientation() == 3) || (x-getX()<0 && getOrientation()  == 1)
	 * 			|			numberOfTurns = 2
	 * 			|		else
	 * 			|			numberOfTurns = 1
	 * 			|	 else if (x-getX()>0 && getOrientation() == 1) || (x-getX()<0 && getOrientation()  == 3) ||
	 * 			|	(y-getY()>0 && getOrientation() == 2) || (y-getY()>0 && getOrientation() == 0) then
	 * 			|		numberOfTurns = 1
	 * 			|	 else
	 * 			|		numberOfTurns = 2
	 * 			| in
	 * 			| 	result == numberOfTurns*getTurnEnergy() + (abs(x - getX()) + abs(y - getY()))*getMoveEnergy()
	 * @throws IllegalArgumentException 
	 * 		The point (x,y) is not reachable (not on the board)
	 * 		| !isValidPosition(x,y) 
	 * 	(or	| (x < 0) || (x > getMaxX() ) || (y < 0) || (y > getMaxY() )  )
	 */
	public double getEnergyRequiredToReach(long x, long y) throws IllegalArgumentException {
		if ( !isValidPosition(x,y) ) {
			throw new MoveOutOfBoardException(this);
		}
		long differenceX = x - getX();
		long differenceY = y - getY();

		//check whether the current orientation is optimal
		boolean inOptimalStartingOrientationX = isInOptimalStartingOrientationX(x,getOrientation());
		boolean inOptimalStartingOrientationY = isInOptimalStartingOrientationY(y,getOrientation());
		
		// the energy required is the Manhattan distance times the moveEnergy (+ the turnEnergy)
		double energy = (Math.abs(differenceX) + Math.abs(differenceY))*getMoveEnergy();
		
		int numberOfTurns;
		if( differenceX == 0 ) {
			numberOfTurns = getNumberOfTurnsNeededToFaceY(y);
		}
		else if(differenceY == 0 ) {
			numberOfTurns = getNumberOfTurnsNeededToFaceX(x);
		}
		else if(inOptimalStartingOrientationX  || inOptimalStartingOrientationY) {
			numberOfTurns = 1;
		}
		else if(!inOptimalStartingOrientationX && !inOptimalStartingOrientationY) {
			numberOfTurns = 2;
		}
		else{ // Logically excluded
			throw new IllegalStateException();
		}
		energy += numberOfTurns*getTurnEnergy();
		return energy;
	}
	/**
	 * Get the number of turns needed to face the y-coordinate of the destination
	 * @param 	destinationY the y-coordinate of the destination
	 * @return 	the amount of turns needed to face the given destination in y-direction
	 */
	private int getNumberOfTurnsNeededToFaceY(long destinationY) {
		int orientation2Turns = (this.getOrientation()+2)%4;
		if(isInOptimalStartingOrientationY(destinationY,getOrientation() ) || (destinationY == getY() )){
			return 0;
		}
		else if(isInOptimalStartingOrientationY(destinationY, orientation2Turns) ) {
			return 2;
		}
		else{
			return 1;
		}
	}
	/**
	 * Get the number of turns needed to face the x-coordinate of the destination
	 * @param 	destinationX 	the desired x-coordinate to face
	 * @return 	the amount of turns needed to face the given destination
	 */
	private int getNumberOfTurnsNeededToFaceX(long destinationX) {
		int orientation2Turns = (this.getOrientation()+2)%4;
		if(isInOptimalStartingOrientationX(destinationX,getOrientation()) || (destinationX == getX()) ){
			return 0;
		}
		else if(isInOptimalStartingOrientationX(destinationX, orientation2Turns) ) {
			return 2;
		}
		else{
			return 1;
		}
	}
	/**
	 * @param 	x  the x-coordinate to face
	 * @param 	orientation  the orientation to test
	 * @return 	true if parameter orientation would be an optimal orientation for this Robot to face x
	 */
	private boolean isInOptimalStartingOrientationX(long x,int orientation) {
		long differenceX = x- this.getX();
		boolean inOptimalStartingOrientationX =false;
		if((differenceX>0 && orientation == 1) || (differenceX<0 && orientation == 3)) {
			inOptimalStartingOrientationX = true;
		}
		return inOptimalStartingOrientationX;
	}
	/**
	 * @param 	y  the y-coordinate to face
	 * @param 	orientation  the orientation to test
	 * @return 	true if parameter orientation would be an optimal orientation for this Robot to face y
	 */
	private boolean isInOptimalStartingOrientationY(long y,int orientation) {
		long differenceY = y- this.getY();
		boolean inOptimalStartingOrientationY =false;
		if((differenceY>0 && orientation == 2) || (differenceY<0 && orientation == 0)) {
			inOptimalStartingOrientationY = true;
		}
		return inOptimalStartingOrientationY;
	}

	/**
	 * Move this Robot and/or the given Robot next to each other or as close as possible. 'robot' should be an instance of Robot
	 * 
	 * @param	robot the robot to move next to
	 * @post 	if robot == this, return
	 * 			| if robot == this then return
	 * @post 	if the robots are at same location, then one of them moves if it has sufficient energy so that they come 
	 * 			next to each other
	 * 			| if 	( (robot.getX() == this.getX()) && (robot.getY() == this.getY()) ) && 
	 *			| 		( (this.getEnergy() >= this.getMoveEnergy() )|| (robot.getEnergy() >= rob.getMoveEnergy() ) )
	 *			| then	abs((new robot).getX() - new.getX() )+abs((new robot).getY() - new.getY() ) == 1
	 * @post 	The robots end up next to each other if they have still energy to move efficiently after exiting the method
	 * 			| if 	( (new.getEnergy() >= this.getMoveEnergy() + 2*this.getTurnEnergy() )|| 
	 * 			|  		( (new robot).getEnergy() >= robot.getMoveEnergy() + 2*robot.getTurnEnergy()) )
	 *			| then	abs((new robot).getX() - new.getX() )+abs((new robot).getY() - new.getY() ) == 1
	 * @post 	The robots have moved to each other as close as possible, optimally, even if they didn't have enough energy. 
	 * 			| if (getMoveEnergy() == robot.getMoveEnergy()) && (getTurnEnergy() == robot.getTurnEnergy())
	 * 			| 	 getEnergy() + robot.getEnergy() - new.getEnergy() -(new robot).getEnergy()  <=
	 * 			|	 (abs(robot.getX() - (new robot).getX() ) + abs(robot.getY() - (new robot).getY() ) + 
	 * 			| 	 abs(getX() - new.getX() ) + abs(getY() - new.getY() )) * getMoveEnergy() + 4*getTurnEnergy() 
	 * @post	The robots have moved to each other as close as possible, optimally.
	 * 			| if (getMoveEnergy() == robot.getMoveEnergy()) && (getTurnEnergy() == robot.getTurnEnergy())
	 * 			| 	 getEnergy() + robot.getEnergy() - new.getEnergy() -(new robot).getEnergy() <=
	 * 			| 	(abs(robot.getX() - getX() ) + abs(robot.getY() - getY() ) ) * getMoveEnergy() + 4*getTurnEnergy() 
	 * @post 	This Robot used the right amount of energy to move and turn. It maximally turned twice, so the absolute value
	 * 		 	of the difference in orientation gives the number of turns.
	 * 			| (abs(getX() - new.getX() ) + abs(getY() - new.getY() ) )*getMoveEnergy() +
	 * 			|  abs( getOrientation() - new.getOrientation()  )*getTurnEnergy()
	 * 			| == getEnergy() - new.getEnergy()
	 * @post 	The other Robot used the right amount of energy to move and turn. It maximally turned twice, so the absolute value
	 * 		 	of the difference in orientation gives the number of turns
	 * 			| (abs(robot.getX() - (new robot).getX() ) + abs(robot.getY() - (new robot).getY() )
	 * 			| )*robot.getMoveEnergy() + abs(robot.getOrientation() - (new robot).getOrientation() )*robot.getTurnEnergy()
	 * 			| == robot.getEnergy() - (new robot).getEnergy()
	 * 
	 * @throws 	NullPointerException
	 * 			The destination robot does not exist
	 * 			| robot == null
	 * @throws 	ClassCastException
	 * 			The destination robot is no Robot
	 * 			| ! (robot instanceOf Robot)
	 */
	public void moveNextTo(IRobot robot) throws NullPointerException, ClassCastException {
		
		Robot rob = (Robot) robot;
		if(this == robot)
			return;
		if ( (robot.getX() == this.getX()) && (robot.getY() == this.getY()) ) {
			if(this.getEnergy() >= this.getMoveEnergy() )
				move();
			// The assumption is that both robots have the same energy cost to move
			else if (robot.getEnergy() >= rob.getMoveEnergy() )
				robot.move();
			return;
		}
		// if one or both are in the optimal orientation, they should move
		moveIfInOptimalDirection(rob);
		rob.moveIfInOptimalDirection(this);

		// after 4 times executed the tryToMoveOptimally method, both robots are as close as possible
		for(byte attempt = 1; (!areNextToEachOther(robot) ) && attempt <= 4; attempt ++) {
			tryToMoveOptimally(rob);
		}
	}
	/**
	 * Try to move this Robot or the other one so that they come closer while making an optimal decision 
	 * about which Robot should turn
	 * @param other  the other Robot
	 */
	private void tryToMoveOptimally(Robot rob) {
		// select the robot with the least required energy to turn and let it turn and move
		if(turnOptimally(rob,false) < rob.turnOptimally(this,false) ) {
			turnOptimally(rob,true);
			this.moveIfInOptimalDirection(rob);
		}
		else{
			rob.turnOptimally(this, true);
			rob.moveIfInOptimalDirection(this);
		}
	}
	/**
	 * @param 	robot  the robot to test
	 * @return 	true if this Robot is next to the given Robot
	 */
	private boolean areNextToEachOther(IRobot robot) {
		return Math.abs(robot.getX() - this.getX() )+Math.abs(robot.getY() - this.getY() ) == 1;
	}
	/**
	 * Does nothing if not in optimal orientation, otherwise moves just enough towards other Robot.
	 * @param other  the other Robot
	 */
	private void moveIfInOptimalDirection(Robot other) {
		// calculate the amount of to be taken to get next to each other
		long numberOfSteps = this.getNumberOfStepsOptimalOrientation(other.getX() - this.getX(), other.getY() - this.getY());
		
		// move as long as there is sufficient energy  and not next to each other 
		for(long step = 1; (step <= numberOfSteps)&&(this.getEnergy() >= this.getMoveEnergy()); step ++) {
			this.move();
		}
	}
	/** 
	 * Get the number of steps needed to move assuming the robot is in an optimal orientation
	 * @param 	differenceX  the relative x-position of the destination w.r.t. this Robot
	 * @param 	differenceY  the relative y-position of the destination w.r.t. this Robot
	 * @return 	-1 if not in optimal orientation, else the required number of steps
	 */
	private long getNumberOfStepsOptimalOrientation(long differenceX, long differenceY) {
		// the 2 robots are next to each other, no steps are required
		if(Math.abs(differenceX)+Math.abs(differenceY) == 1)
			return 0;
		if ((differenceY>0 && orientation == 2) || (differenceY<0 && orientation == 0)) {
			if (differenceX == 0)
				// 1 step less than the differenceY is required because  they have the same x-coordinate
				return Math.abs(differenceY)-1;
			else
				return Math.abs(differenceY);
		}
		if((differenceX>0 && orientation == 1) || (differenceX<0 && orientation == 3)) {
			if (differenceY == 0)
					// 1 step less than the differenceX is required because  they have the same y-coordinate
					return Math.abs(differenceX)-1;
			else
				return Math.abs(differenceX);
		}
		// the robot is not in an optimal orientation
		return -1;
	}
	/**
	 * Does nothing if already in good orientation, otherwise turns it in an optimal orientation. 
	 * The boolean actuallyTurn is used to indicate if the robot should actually turn or only calculate
	 * the energy to required to do so.
	 * @param 	other the other Robot
	 * @param 	actuallyTurn whether the robot should turn or only calculate the energy to do so
	 * @return 	how much energy it would require to turn, Double.MAX_VALUE if not enough energy to 
	 * 			turn AND move ,or already next to each other
	 */
	private double turnOptimally(Robot other, boolean actuallyTurn) {
		// the robot can not turn and move so cannot move closer to the other robot
		if(getEnergy() < getTurnEnergy() + getMoveEnergy() )
			return Double.MAX_VALUE;
		// check if one of the robots are in the optimal orientation after 1 turn clockwise
		int orientation1turnClockwise = (this.getOrientation() +1)%4;

		if( (isInOptimalStartingOrientationX(other.getX(), orientation1turnClockwise)) || (isInOptimalStartingOrientationY(other.getY(), orientation1turnClockwise))) {
			// only turn if the user wises to do so
			if (actuallyTurn)
				turnClockwise();
			return getTurnEnergy();
		}
		// check if one of the robots are in the optimal orientation after 1 turn counterclockwise
		int orientation1turnCounterClockwise = (this.getOrientation() +3)%4;
		if( (isInOptimalStartingOrientationX(other.getX(), orientation1turnCounterClockwise)) || (isInOptimalStartingOrientationY(other.getY(), orientation1turnCounterClockwise))) {
			// only turn if the user wises to do so			
			if (actuallyTurn)
				turnCounterClockwise();
			return getTurnEnergy();
		}
		// the robot can not turn 2 times and move so cannot move closer to the other robot
		if(getEnergy() < 2*getTurnEnergy() + getMoveEnergy() )
			return Double.MAX_VALUE;
		int orientation2turns = (this.getOrientation() +2)%4;
		// check if one of the robots are in the optimal orientation after 2 turns
		if( (isInOptimalStartingOrientationX(other.getX(), orientation2turns)) || (isInOptimalStartingOrientationY(other.getY(), 
				orientation2turns))) {
			// only turn if the user wises to do so		
			if (actuallyTurn) {
				turnCounterClockwise();
				turnCounterClockwise();
			}
			return 2*getTurnEnergy();
		}
		return 0.0d;
	}
}
